#include "./stdp.h"
static void stdp_solve(Format_FieldStruct* msg, LinkCfgStruct* link);

static Stdp_ListenerStruct* Listeners[STDP_LISTENERS_LEN_MAX];
static uint16_t ListenersLen = 0;

void stdp_start() {
  if (fiip_getProtocol(FIIP_PROTOCOL_TYPE_STDP) == NULL) {
    ProtocolStruct protocol;
    protocol.type = FIIP_PROTOCOL_TYPE_STDP;
    protocol.solve = stdp_solve;
    fiip_addProtocol(FIIP_PROTOCOL_TYPE_STDP, &protocol);
  }
}
void stdp_addListener(uint16_t cmd, void (*fun)(Stdp_FieldStruct* msg)) {
  for (int i = 0; i < ListenersLen; i++) {
    if (Listeners[i] != NULL) {
      if (Listeners[i]->cmd == cmd) {
        INFO("stdp listener %04X has been replaced.\n", cmd);
        Listeners[i]->cmd = cmd;
        Listeners[i]->fun = fun;
        return;
      }
    }
  }

  if (ListenersLen < STDP_LISTENERS_LEN_MAX) {
    Listeners[ListenersLen] =
        (Stdp_ListenerStruct*)mempool_alloc(sizeof(Stdp_ListenerStruct));
    Listeners[ListenersLen]->cmd = cmd;
    Listeners[ListenersLen]->fun = fun;
    ListenersLen++;
  }
}
void stdp_solve(Format_FieldStruct* fiipMsg, LinkCfgStruct* link) {
  Stdp_FieldStruct* msg = (Stdp_FieldStruct*)fiipMsg;
  Stdp_ListenerStruct* lis_ffff = NULL;
  Stdp_ListenerStruct* lis_this = NULL;

  for (uint16_t i = 0; i < ListenersLen; i++) {
    if (Listeners[i]->cmd == 0xFFFF) {
      lis_ffff = Listeners[i];
    }
    if (Listeners[i]->cmd == (msg->cmd[0] << 8 | msg->cmd[1])) {
      lis_this = Listeners[i];
    }
  }
  if (lis_ffff) {
    lis_ffff->fun(msg);
  }
  if (lis_this && lis_this != lis_ffff) {
    if (lis_this->cmd == (msg->cmd[0] << 8 | msg->cmd[1])) {
      lis_this->fun(msg);
    }
  } else {
    if (!lis_ffff)
      WARN("stdp no listener 0xffff.");
    else
      WARN("stdp no listener 0x%04X.", (msg->cmd[0] << 8) + msg->cmd[1]);
  }
}
